package upm.etsit.gsi.lejos.mazesolver.behaviours;

import upm.etsit.gsi.lejos.mazesolver.DataExchange;
import lejos.robotics.navigation.TachoPilot;
import lejos.robotics.subsumption.Behavior;

/**
 * Comportamiento inicial, se encartga de ahcer avanzar al robot
 * @author jrubia
 */
public class GoStraight implements Behavior {

    /**
     * Piloto que controla el movimiento del robot.
     */
    private TachoPilot pilot;
    /**
     * Intercambiador de datos.
     */
    private DataExchange DE;
    /**
     * Mapeador.
     */
    private Mapper mapper;

    /**
     * Constructor con parámetros
     *
     * @param pilot
     * @param DE
     * @param mapeador
     */
    public GoStraight(TachoPilot pilot, DataExchange DE, Mapper mapper) {
        this.pilot = pilot;
        this.DE = DE;
        this.mapper = mapper;
    }

    /**
     * Toma el control cuando DE.direction es 0
     *
     * @return
     */
    public boolean takeControl() {
        return (DE.getDirection() == 0 && !DE.isExploracion());
    }

    /**
     * Mapea la posicion en la que se encuentra, avanza 40 cm., llama al mapeador para que se situe en la nueva posición y pone DE.direction a 1 para que tome el control TurnLeft
     * Si detecta que se va a chocar, detiene el movimiento y continua con el proceso anterior, aunque a efectos de mapeo, será como si hubiera avanzado los 40 cm.
     */
    public void action() {

        this.mapper.map();

        if (!mapper.pasadoSiguiente()) {
            this.pilot.travel(DE.getDIS_SEG()*10, true);

            while (pilot.isMoving()) {
                if (DE.getDistance() < 10) {
                    pilot.stop();
                }
            }

            this.mapper.advance();
            this.DE.setDirection(1);
        } else {
            DE.setExploration(true);
        }
    }

    /**
     *  Cuando deja de tener el control, se para para realizar las comprobaciones
     */
    public void suppress() {
        this.pilot.stop();
    }
}

